#include "car_step_helper.hpp"

void stand_pick(int i, int j, int color, double &angle_t, double &claw_t)
{
    Pcar->set_rotate_pos(angle_t, 100, 200, zdt_motor::motor_pos_mode_absolute);
    Pcar->set_bracket_pos(-claw_t, 100, 200, zdt_motor::motor_pos_mode_absolute, true);
    pick_obj(j, 0);
}

double adjust_angle(double yaw, double angle)
{
    double temp_a = 90 - fabs(angle);
    if (angle < 0)
    {
        return yaw - temp_a;
    }
    else
    {
        return yaw + temp_a;
    }
}

void stand_put(int i, int j, int color, int h, double &angle_t, double &claw_t)
{
    double angle, claw_y;

    Pcar->set_bracket_pos(BRACKET_TARGET_RANEG + 57.5 * abs(i - 1), 100, 200, zdt_motor::motor_pos_mode_absolute);
    Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE - 42 * (i - 1), 100, 200, zdt_motor::motor_pos_mode_absolute, true);
    start_trace_color_with_claw((camera::camera_color_en)color, 500000, 15);
    get_claw_trace_data(angle, claw_y);
    angle_t = angle;
    claw_t = claw_y;
    put_obj(j, h, claw_y, angle);
}

int step_get_color(int color_area_zone)
{
    if (!cam)
        return -1;

    return cam->get_color(color_area_zone);
}
void get_ground_color(int get_ground_color[])
{
    get_ground_color[1] = step_get_color();
    std::this_thread::sleep_for(std::chrono::milliseconds(800));
    Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE + 42, 100, 200, zdt_motor::motor_pos_mode_absolute);
    Pcar->set_bracket_pos(167.5, 100, 200, zdt_motor::motor_pos_mode_absolute);
    std::this_thread::sleep_for(std::chrono::milliseconds(1500));
    get_ground_color[0] = step_get_color();
    std::this_thread::sleep_for(std::chrono::milliseconds(800));
    get_ground_color[2] = 6 - get_ground_color[0] - get_ground_color[1];
    Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute);
    Pcar->set_bracket_pos(110, 100, 200, zdt_motor::motor_pos_mode_absolute);
    std::this_thread::sleep_for(std::chrono::milliseconds(1500));
}

void reset_mission()
{
    Pcar->reset_lift_pos();                                                               // 升降回零
    Pcar->set_claw_channel(1, 70);                                                        // 收起夹爪防止干涉
    Pcar->reset_rotate_pos();                                                             // 转台回零     // 转台初始化
    Pcar->reset_bracket_pos();                                                            // 机械臂回零
    Pcar->set_lift_pos(LIFT_ROTATE_HEIGHT, 500, 200, zdt_motor::motor_pos_mode_absolute); // 滑台初始化

    Pcar->set_rotate_pos(ROTATE_TRAY0_ANGLE, 80, 240, zdt_motor::motor_pos_mode_absolute);
    Pcar->set_bracket_pos(BRACKET_TRAY0_RANEG - 10, 150, 125, zdt_motor::motor_pos_mode_absolute);
    Pcar->set_claw_channel(1, 45); //  转台初始化出发位置
}
bool pick_obj(int obj, int height)
{
    Pcar->set_claw_channel(1, CLAW_MAX_ANGLE); // 释放物料
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
    // Pcar->set_lift_pos(LIFT_ROTATE_HEIGHT, 5000, 255, zdt_motor::motor_pos_mode_absolute);     // 保证上升至旋转高度
    // Pcar->set_bracket_pos(BRACKET_TARGET_RANEG, 150, 125, zdt_motor::motor_pos_mode_absolute); // 保证移动至靶标
    // Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 5000, 245, zdt_motor::motor_pos_mode_absolute);   // 保证旋转至正右侧

    // std::this_thread::sleep_for(std::chrono::milliseconds(500));

    switch (height)
    {
    case 0:
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        Pcar->set_lift_pos(LIFT_GND_HEIGHT + 5, 1000, 0, zdt_motor::motor_pos_mode_absolute, true); // 下降至地面高度
        // std::this_thread::sleep_for(std::chrono::milliseconds(100));
        Pcar->set_claw_channel(1, CLAW_MIN_ANGLE); // 抓取物料
        std::this_thread::sleep_for(std::chrono::milliseconds(200));

        break;
    case 1:
        Pcar->set_lift_pos(LIFT_TABLE_HEIGHT + 5, 1000, 254, zdt_motor::motor_pos_mode_absolute, true); // 下降至转盘高度
        // std::this_thread::sleep_for(std::chrono::milliseconds(100));
        Pcar->set_claw_channel(1, CLAW_MIN_ANGLE); // 抓取物料
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
        break;
    default:
        break;
    }

    Pcar->set_lift_pos(LIFT_ROTATE_HEIGHT, 5000, 255, zdt_motor::motor_pos_mode_absolute, true); // 上升至旋转高度
    // std::this_thread::sleep_for(std::chrono::milliseconds(800));
    switch (obj)
    {
    case 0:
        Pcar->set_bracket_pos(BRACKET_TRAY0_RANEG, 150, 200, zdt_motor::motor_pos_mode_absolute); // 移动至物料盘0
        std::this_thread::sleep_for(std::chrono::milliseconds(400));
        Pcar->set_rotate_pos(ROTATE_TRAY0_ANGLE, 5000, 240, zdt_motor::motor_pos_mode_absolute, true); // 旋转至物料盘0
        break;
    case 1:
        Pcar->set_bracket_pos(BRACKET_TRAY1_RANEG, 150, 200, zdt_motor::motor_pos_mode_absolute); // 移动至物料盘1
        std::this_thread::sleep_for(std::chrono::milliseconds(400));
        Pcar->set_rotate_pos(ROTATE_TRAY1_ANGLE, 5000, 240, zdt_motor::motor_pos_mode_absolute, true); // 旋转至物料盘1
        break;
    case 2:
        Pcar->set_bracket_pos(BRACKET_TRAY2_RANEG, 150, 200, zdt_motor::motor_pos_mode_absolute); // 移动至物料盘2
        std::this_thread::sleep_for(std::chrono::milliseconds(400));
        Pcar->set_rotate_pos(ROTATE_TRAY2_ANGLE, 5000, 240, zdt_motor::motor_pos_mode_absolute, true); // 旋转至物料盘2
        break;
    default:
        break;
    }
    // std::this_thread::sleep_for(std::chrono::milliseconds(1500));
    Pcar->set_lift_pos(LIFT_TRAY_HEIGHT, 5000, 254, zdt_motor::motor_pos_mode_absolute, true); // 下降至物料盘高度
    // std::this_thread::sleep_for(std::chrono::milliseconds(200));
    Pcar->set_claw_channel(1, CLAW_MAX_ANGLE);                                                   // 释放物料
    Pcar->set_lift_pos(LIFT_ROTATE_HEIGHT, 5000, 254, zdt_motor::motor_pos_mode_absolute, true); // 上升至旋转高度
    // std::this_thread::sleep_for(std::chrono::milliseconds(400));
    Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 5000, 240, zdt_motor::motor_pos_mode_absolute);         // 旋转至正右侧
    Pcar->set_bracket_pos(BRACKET_TARGET_RANEG, 150, 200, zdt_motor::motor_pos_mode_absolute, true); // 伸展至靶标上方
    // std::this_thread::sleep_for(std::chrono::milliseconds(200));
    Pcar->set_claw_channel(1, 70);
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
bool put_obj(int obj, int height, double claw_y, double rotate_angle)
{
    Pcar->set_claw_channel(1, CLAW_MAX_ANGLE);

    Pcar->set_lift_pos(LIFT_ROTATE_HEIGHT, 5000, 254, zdt_motor::motor_pos_mode_absolute, true); // 保证处于旋转高度
    // std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    // 旋转至各个物料盘
    switch (obj)
    {
    case 0:
        Pcar->set_bracket_pos(BRACKET_TRAY0_RANEG, 5000, 200, zdt_motor::motor_pos_mode_absolute);     // 移动至物料盘0
        Pcar->set_rotate_pos(ROTATE_TRAY0_ANGLE, 5000, 240, zdt_motor::motor_pos_mode_absolute, true); // 旋转至物料盘0
        break;
    case 1:
        Pcar->set_bracket_pos(BRACKET_TRAY1_RANEG, 5000, 200, zdt_motor::motor_pos_mode_absolute);     // 移动至物料盘1
        Pcar->set_rotate_pos(ROTATE_TRAY1_ANGLE, 5000, 240, zdt_motor::motor_pos_mode_absolute, true); // 旋转至物料盘1
        break;
    case 2:
        Pcar->set_bracket_pos(BRACKET_TRAY2_RANEG, 5000, 200, zdt_motor::motor_pos_mode_absolute);     // 移动至物料盘2
        Pcar->set_rotate_pos(ROTATE_TRAY2_ANGLE, 5000, 240, zdt_motor::motor_pos_mode_absolute, true); // 旋转至物料盘2
        break;
    default:
        break;
    }

    // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    Pcar->set_claw_channel(1, CLAW_MAX_ANGLE); // 夹爪略微张开
    // std::this_thread::sleep_for(std::chrono::milliseconds(500));
    Pcar->set_lift_pos(LIFT_TRAY_HEIGHT, 5000, 254, zdt_motor::motor_pos_mode_absolute, true); // 下降至物料盘高度
    // std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    Pcar->set_claw_channel(1, CLAW_MIN_ANGLE);
    std::this_thread::sleep_for(std::chrono::milliseconds(200));                           // 抓取物料
    Pcar->set_lift_pos(LIFT_ROTATE_HEIGHT, 5000, 254, zdt_motor::motor_pos_mode_absolute); // 上升至旋转高度
    std::this_thread::sleep_for(std::chrono::milliseconds(400));
    Pcar->set_bracket_pos(BRACKET_AVOID_RANGE, 5000, 200, zdt_motor::motor_pos_mode_absolute, true); // 伸展至放置长度
    // std::this_thread::sleep_for(std::chrono::milliseconds(400));
    Pcar->set_bracket_pos(-claw_y, 5000, 200, zdt_motor::motor_pos_mode_absolute); // 伸展至放置长度
    std::this_thread::sleep_for(std::chrono::milliseconds(400));
    Pcar->set_rotate_pos(rotate_angle, 5000, 240, zdt_motor::motor_pos_mode_absolute, true); // 旋转至正右侧//旋转至正右侧
    // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    // 下降至两种高度
    switch (height)
    {
    case 0:
        Pcar->set_lift_pos(LIFT_GND_HEIGHT, 5000, 254, zdt_motor::motor_pos_mode_absolute, true); // 下降至地面高度
        // std::this_thread::sleep_for(std::chrono::milliseconds(2000));
        break;
    case 1:
        Pcar->set_lift_pos(LIFT_OBJ_HEIGHT, 5000, 254, zdt_motor::motor_pos_mode_absolute, true); // 下降至物料高度
        // std::this_thread::sleep_for(std::chrono::milliseconds(1500));
        break;
    default:
        break;
    }
    Pcar->set_claw_channel(1, CLAW_MAX_ANGLE); // 释放物料
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
    Pcar->set_lift_pos(LIFT_ROTATE_HEIGHT, 5000, 254, zdt_motor::motor_pos_mode_absolute, true); // 重新上升至旋转高度
    // std::this_thread::sleep_for(std::chrono::milliseconds(1300));
    Pcar->set_claw_channel(1, 70);
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
